//#define _CRTDBG_MAP_ALLOC


#include "StdAfx.h"
#include "SURFeatures.h"
//#define _CRTDBG_MAP_ALLOC

extern "C" float getDPDistance(float* descriptorsMinArray,int minSize,float* descriptorsMaxArray, int maxSize);
 bool sortAssending (double i,double j) { return (i<j); }
SURFeatures::SURFeatures(void)
{  
	//colors[]={0,0,255};
	//colors = (((0,0,255)),((0,128,255)),((0,255,255)),((0,255,0)),((255,128,0)),((255,255,0)),((255,0,0)),((255,0,255)),((255,255,255)));
	 /*this->colors[] =
	{
        {{0,0,255}},
        {{0,128,255}},
        {{0,255,255}},
        {{0,255,0}},
        {{255,128,0}},
        {{255,255,0}},
        {{255,0,0}},
        {{255,0,255}},
        {{255,255,255}}
    };*/
	 /*objectKeypoints = 0;
	 objectDescriptors = 0;
     imageKeypoints = 0;
	 imageDescriptors = 0;  */
	/*storage = cvCreateMemStorage(0);  used with CPU
	 params = cvSURFParams(500, 1);*/

	 //src_corners[4] = {{0,0},{this->object->width,0},{this->object->width,this->object->height},{0,this->object->height}};
	 /**src_corners =new CvPoint[4];
	 dst_corners=new CvPoint[4];*/

	// src_corners =(CvPoint*) malloc(4);
	//dst_corners = (CvPoint*) malloc(4);

	

}

void SURFeatures::setObject(IplImage* object)
{
	this->object=object;
}
void SURFeatures::setImage(IplImage* image)
{
	this->image=image;
}

void SURFeatures::setSrcCorners()
{
	 src_corners[0] =cvPoint(0,0);
	 src_corners[1] =cvPoint(this->object->width,0);
	  //src_corners[1] =cvPoint(0,0);
	 src_corners[2] =cvPoint(this->object->width,this->object->height);
	 src_corners[3] =cvPoint(0,this->object->height);

}

cv::gpu::GpuMat SURFeatures::getObjectKeyPointsGpuMat()
{
    return this->objectKeyPointsGpuMat;
}
cv::gpu::GpuMat SURFeatures::getObjectDescriptorsGpuMat()
{
    return this->objectDescriptorsGpuMat;
}
cv::gpu::GpuMat SURFeatures::getImageKeyPointsGpuMat()
{
    return this->objectKeyPointsGpuMat;
}
cv::gpu::GpuMat SURFeatures::getImageDescriptorsGpuMat()
{
    return this->imageDescriptorsGpuMat;
}
CvSeq* SURFeatures::getObjectDescriptors()
{
	return this->objectDescriptors;
}
CvSeq* SURFeatures::getImageDescriptors()
{
	return this->imageDescriptors;
} 
void SURFeatures::extractSURF(int objectImage)
{
	//IplImage *source=this->object;  if 
	if(objectImage==0)
	{
		//source=this->object;
		cvExtractSURF( this->object, 0, &objectKeypoints, &objectDescriptors, storage, params );
	}
	else if(objectImage==1)
	{
		//source=this->image;
		cvExtractSURF( this->image, 0, &imageKeypoints, &imageDescriptors, storage, params );
	}
	//cvExtractSURF( source, 0, &objectKeypoints, &objectDescriptors, storage, params );
} 

void SURFeatures::extractSURFGPU(int objectImage)
{
    //cv::gpu::SURF_GPU surfGpu;// = new SURF_GPU();
  //  objectKeyPointsGpuMat.~GpuMat();
  //  objectDescriptorsGpuMat.~GpuMat();
    //https://gist.github.com/1451463
  //https://code.ros.org/trac/opencv/browser/trunk/opencv/samples/gpu/matcher_simple.cpp?rev=4403
	//IplImage *source=this->object;  if 
	if(objectImage==0)
	{
	    //cv::gpu::SURF_GPU* surfGpu = new  cv::gpu::SURF_GPU();
	    //cv::gpu::SURF_GPU surfGpu;
	    cv::Mat objectMat = cv::Mat(this->object,false);
	    cv::gpu::GpuMat objectGpuMat;//,maskGpuMat;    
	    objectGpuMat.upload(objectMat);
	
	    
	  
	    
	    //surfGpu(objectGpuMat,maskGpuMat,objectKeyPointsGpuMat,objectDescriptorsGpuMat);
	     surfGpu.upright=true;
	    surfGpu(objectGpuMat,cv::gpu::GpuMat(),objectKeyPointsGpuMat,objectDescriptorsGpuMat);
	  
	   
	    	
		/*  need if the device copy of the keypoints and descriptors are used     */
		keypointsObject.clear();
		descriptorsObject.clear();		
		 surfGpu.downloadKeypoints(objectKeyPointsGpuMat, keypointsObject);
		 surfGpu.downloadDescriptors(objectDescriptorsGpuMat, descriptorsObject);
		 
		
		 objectMat.~Mat();
	     objectGpuMat.~GpuMat();
	//	maskGpuMat.~GpuMat();
		//surfGpu->releaseMemory();
		      
		 //keypointsImage._D
	  // descriptorImage.
	/*  keypointsObject.clear();
		descriptorsObject.clear();  */
		
	}
	else if(objectImage==1)
	{
	    //cv::gpu::SURF_GPU surfGpu;
	    cv::Mat imageMat = cv::Mat(this->image,false);
	    cv::gpu::GpuMat imageGpuMat;//,maskGpuMat;	    
	    imageGpuMat.upload(imageMat);
	    
	    
	    surfGpu.upright=true;
	    surfGpu(imageGpuMat,cv::gpu::GpuMat(),imageKeyPointsGpuMat,imageDescriptorsGpuMat);
	    
	    
	  /* need if the device copy of the keypoints and descriptors are used  */
	   keypointsImage.clear();
	   descriptorImage.clear();
	   surfGpu.downloadKeypoints(imageKeyPointsGpuMat, keypointsImage); 
       surfGpu.downloadDescriptors(imageDescriptorsGpuMat, descriptorImage); 
       
     
       imageMat.~Mat();
       imageGpuMat.~GpuMat();
	  // maskGpuMat.~GpuMat();
	    
	  // surfGpu.releaseMemory();   
	     
	   // cout<<"No of Keypoints"<<keypointsImage.size()<<endl;
	   
		//source=this->image;
		//cvExtractSURF( this->image, 0, &imageKeypoints, &imageDescriptors, storage, params );
	}

   // vector<DMatch> matches;
   
 
	//cvExtractSURF( source, 0, &objectKeypoints, &objectDescriptors, storage, params );
	//surfGpu->releaseMemory();
	//surfGpu.releaseMemory();
}

void SURFeatures::releaseSURFGPU(int objectImage)
{
   if(objectImage==0)
   {
        objectKeyPointsGpuMat.~GpuMat();
	    objectDescriptorsGpuMat.~GpuMat();       
   }
   else if(objectImage==1)
   {
        imageKeyPointsGpuMat.~GpuMat();
        imageDescriptorsGpuMat.~GpuMat();   
   }
}    
 int SURFeatures::getKeypointsObjectSize()
 {
    return keypointsObject.size(); 
 }
 int SURFeatures::getKeypointsImageSize()
 {
    return keypointsImage.size(); 
 }
  int SURFeatures::getDescriptorObjectSize()
 {
    return descriptorsObject.size(); 
 }
 int SURFeatures::getDescriptorImageSize()
 {
    return descriptorImage.size(); 
 } 
 
 vector<float> SURFeatures::getDescriptorsObject()
 {
      return    descriptorsObject;
 }
 vector<float> SURFeatures::getDescriptorImage()
 {
      return    descriptorImage;
 }
 /*void SURFeatures::matchSURFCPU()
 {
    cv::DescriptorMatcher::match(
    
 } */
float SURFeatures::matchSURFGPU()
{
     // matching descriptors
    
   // cv::gpu::GpuMat trainIdx, distance;
   // matcher.matchSingle(objectDescriptorsGpuMat,imageDescriptorsGpuMat,trainIdx, distance);
    
   /* keypointsObject.clear();
	descriptorsObject.clear();
	
     surfGpu.downloadKeypoints(objectKeyPointsGpuMat, keypointsObject);
     surfGpu.downloadDescriptors(objectDescriptorsGpuMat, descriptorsObject);
    
    keypointsImage.clear();
   descriptorImage.clear();
   surfGpu.downloadKeypoints(imageKeyPointsGpuMat, keypointsImage);    
   surfGpu.downloadDescriptors(imageDescriptorsGpuMat, descriptorImage); */
     
    
   // vector<DMatch> matches;
     // matcher.matchDownload(trainIdx, distance, matches);
      
     

    /* Use Match single and chack
     use CPU match, not bruteforce      page 394
     if there is an improvement then try to impliment DP match on GPU*/
      float returnFloat=0;
      int noOfFeaturesInObject =((cv::Mat)objectDescriptorsGpuMat).rows; 
    matcher.match(objectDescriptorsGpuMat, imageDescriptorsGpuMat,matchesOfTwo, cv::gpu::GpuMat());
    int goodMatchCount =0;
    
    
    
    float sumDistance  =0;
     for(int p = 0; p < noOfFeaturesInObject;p++)
     { 
        sumDistance = sumDistance + matchesOfTwo[p].distance;
        /*if( matchesOfTwo[p].distance <0.55 )
        {
            goodMatchCount++;                   
        }*/
       // goodMatchCount++;
     } 
    // returnFloat = goodMatchCount/(float)noOfFeaturesInObject;
    returnFloat=-1;
    if(noOfFeaturesInObject>0 )
    {
        returnFloat = sumDistance/noOfFeaturesInObject;
     }
    
     /*
     float returnFloat=0;
     if(  objectDescriptorsGpuMat.rows>0&&imageDescriptorsGpuMat.rows>0)
     {
         matcherFB.match( (cv::Mat)objectDescriptorsGpuMat,(cv::Mat)imageDescriptorsGpuMat, matchesFB );
        
         int goodMatchCount =0;
         for(int p = 0; p < ((cv::Mat)objectDescriptorsGpuMat).rows;p++)
         { 
            if( matchesFB[p].distance <0.4 )
            {
                goodMatchCount++;                   
            }
         } 
         
         returnFloat = goodMatchCount/(float)((cv::Mat)objectDescriptorsGpuMat).rows;
         if( returnFloat>0.3)
         {
             printf("Images are matched %.2f\n", returnFloat);
         }
     }*/
    
      
   // cv::gpu::BruteForceMatcher_GPU<L2<float>>::matchDownload(trainIdx, distance, matches);
    
    //cv::gpu::BruteForceMatcher_GPU<L2<float>>::
      // drawing the results
   /* Mat img_matches;
    drawMatches(Mat(this->image), keypointsObject, Mat(this->object), keypointsImage, matchesOfTwo, img_matches);
    
    namedWindow("matches", 0);
    imshow("matches", img_matches);
    waitKey(0);*/      

    objectKeyPointsGpuMat.~GpuMat();
	objectDescriptorsGpuMat.~GpuMat();

    imageKeyPointsGpuMat.~GpuMat();
    imageDescriptorsGpuMat.~GpuMat();   
    
   // trainIdx.~GpuMat();
   // distance.~GpuMat (); 
      
     // matcher.clear();
     
     /*cout<<endl;
        for(int j=0;j<matchesOfTwo.size();j++)
        {
            cout<<matchesOfTwo[j].distance<<" ";
        }         
        cout<<endl;*/
        
         
          
    //int noOfMatchers =  matchesOfTwo.size();
    //// return (int)matches.size();
    //cout<<"No of Match at SURF :"<<noOfMatchers<<endl;
    
    
    matchesFB.clear();
    
    return returnFloat;
}

float* SURFeatures::matchSURFGPU(vector<IplImage*> imageVector)
//float* SURFeatures::matchSURFGPU(vector<int> imageIDVector)
{
    int vectorSize =  imageVector.size();
    float* returnFloat=new float[vectorSize];
    
    //cv::gpu::GpuMat trainIdx,imgIdx,distance,allDist,trainCollection;  
   // cv::gpu::GpuMat trainIdx,imgIdx;
   /* vector<cv::gpu::GpuMat> descCollection;*/
    

    //makeGpuCollection(trainCollection, GpuMat& maskCollection,const vector<GpuMat>&masks=std::vector<GpuMat>())
    
      
      // FlannBasedMatcher* fbMatcher=new FlannBasedMatcher();
      printf("\n%d\n",vectorSize);
    for (int i=0; i<vectorSize; i++)
    {
        
       //Sleep(1500); 
        
        //matches.clear();
        
        this->setImage(imageVector[i]);
        this->extractSURFGPU(1);  
        
       // cvShowImage("From Vector",imageVector[i]);
        //cvWaitKey(5);
        
       /* for(int i=0;i<this->descriptorsObject.size();i++)
        {
            cout<<"Object : "<<i<<": "<< this->descriptorsObject[i]<<endl;
        }
        
        for(int i=0;i<this->descriptorImage.size();i++)
        {
            cout<<" Image : "<<i<<": "<< this->descriptorImage[i]<<endl;
        }*/
        
       // this->keypointsObject
       
      /* for(int i=0;i<this->keypointsObject.size();i++)
        {
            //this->keypointsObject[i] ;
            cout<<"X : "<<this->keypointsObject[i].pt.x<<" Y : "<<this->keypointsObject[i].pt.y<<" Octave : "<<this->keypointsObject[i].octave<< endl;
        }    */
           
       // returnFloat[i]=this->dpMatchSURFGPUCPU(this->descriptorsObject,this->descriptorImage); 
        //returnFloat[i]=this->dpMatchSURFGPUCPU(); 
       // returnFloat[i]=10.0; 
        printf("\n%d\n",i);
       if(i==50)
       {
         printf("\n%d\n",i);
         }
        
        //Sleep(1000);
       // matchSingle(const GpuMat& queryDescs, const GpuMat& trainDescs, GpuMat& trainIdx, GpuMat& distance,const GpuMat& mask=GpuMat())
      // match(const GpuMat& queryDescs, const GpuMat& trainDescs, std::vector<DMatch>& matches, const GpuMat&mask=GpuMat())
        //matcher.match(objectDescriptorsGpuMat, imageDescriptorsGpuMat,matches, cv::gpu::GpuMat());
       // matcher.knnMatch(objectDescriptorsGpuMat, imageDescriptorsGpuMat, matches, 5, cv::gpu::GpuMat(),false);
             //matcher.knnMatch(objectDescriptorsGpuMat, imageDescriptorsGpuMat, trainIdx, distance,allDist, 5, cv::gpu::GpuMat());
             
          
        
    //    objectKeyPointsGpuMat
      //  matchesFB.clear();
          int minNoKeypoints=1;
          int maxNoKeypoints=1;
      
          if(((cv::Mat)objectDescriptorsGpuMat).rows < ((cv::Mat)imageDescriptorsGpuMat).rows)
          {
                matcherFB.match( (cv::Mat)objectDescriptorsGpuMat,(cv::Mat)imageDescriptorsGpuMat, matchesFB );
                minNoKeypoints = ((cv::Mat)objectDescriptorsGpuMat).rows;
                maxNoKeypoints = ((cv::Mat)imageDescriptorsGpuMat).rows;
                
          }
          else
          {
                matcherFB.match( (cv::Mat)imageDescriptorsGpuMat,(cv::Mat)objectDescriptorsGpuMat, matchesFB );
                 minNoKeypoints = ((cv::Mat)imageDescriptorsGpuMat).rows;
                maxNoKeypoints = ((cv::Mat)objectDescriptorsGpuMat).rows;
          }
      
          //matcherFB.match( (cv::Mat)objectDescriptorsGpuMat,(cv::Mat)imageDescriptorsGpuMat, matchesFB );
       // matcher.match(objectDescriptorsGpuMat, imageDescriptorsGpuMat,matchesOfTwo, cv::gpu::GpuMat());
        //matcher.matchSingle(objectDescriptorsGpuMat, imageDescriptorsGpuMat,trainIdx,distance, cv::gpu::GpuMat());
        //matcher.matchDownload(trainIdx, distance, matchesOfTwo);
      //  returnFloat[i]=matchesOfTwo.size();
        
        //page 401
        //FlannBasedMatcher* fbMatcher=new FlannBasedMatcher();
       // fbMatcher->match(
//        cv::Mat* descriptorImage = new cv::Mat(descriptorImage); //descriptorsObject
      //  fbMatcher->add(cv::Mat(descriptorImage));
       
        // descCollection.push_back(imageDescriptorsGpuMat);
         cvShowImage("Image",imageVector[i]); 
         cvWaitKey(1);
        cout<<endl;
        //for(int j=0;j<matchesOfTwo.size();j++)
        {
            //cout<<"BF :"<<matchesOfTwo[j].distance<<" FBackKNN :"<<matchesFB[j].distance<<endl;
        } 
        
       // double max_dist = 0; double min_dist = 1000;

      //-- Quick calculation of max and min distances between keypoints
     /* for( int i = 0; i < ((cv::Mat)objectDescriptorsGpuMat).rows; i++ )
      { double dist = matchesFB[i].distance;
        if( dist < min_dist ) min_dist = dist;
        if( dist > max_dist ) max_dist = dist;
      }

      printf("-- Max dist : %f \n", max_dist );
      printf("-- Min dist : %f \n", min_dist ); */  
      
      int goodMatchCount =0;
       //std::vector< DMatch > good_matches;

      for( int p = 0; p < minNoKeypoints;p++ )
      { 
        //if( matchesFB[i].distance < 2*min_dist )
       /* if(p==214)
        {
                printf("\n%d\n",p);
        }*/
        if( matchesFB[p].distance <0.4 )
        {
            goodMatchCount++;
            //good_matches.push_back( matchesFB[p]); 
        }
        
      }  
      
      //-- Draw only "good" matches
      /*Mat img_matches;
      drawMatches(cv::Mat(object,false), keypointsObject, cv::Mat(imageVector[i],false), keypointsImage,
                   good_matches, img_matches, Scalar::all(-1), Scalar::all(-1),
                   vector<char>(), DrawMatchesFlags::NOT_DRAW_SINGLE_POINTS );

      //-- Show detected matches
      imshow( "Good Matches", img_matches );   
      cvWaitKey(0); */
       // cout<<endl;
       returnFloat[i] = goodMatchCount/(float)minNoKeypoints;
      // good_matches.clear();
       printf("Images are matched %.2f", returnFloat[i]);
      if( returnFloat[i]>0.7)
      {
        // printf("Images are matched %.2f", returnFloat[i]);
      }
     /*for( int i = 0; i < good_matches.size(); i++ )
    { printf( "-- Good Match [%d] Keypoint 1: %d  -- Keypoint 2: %d  Error: %.2f\n", i, good_matches[i].queryIdx, good_matches[i].trainIdx,good_matches[i].distance ); }
     */
        
        /*Mat img_matches;
        drawMatches(Mat(this->image), keypointsObject, Mat(this->object), keypointsImage, matchesOfTwo, img_matches);
    
        namedWindow("matches", 0);
        imshow("matches", img_matches);
        waitKey(0);*/ 
         //get the amount of free memory on the graphics card  
      /*  size_t free;  
        size_t total;  
        CUresult result = cuMemGetInfo(&free, &total);  
        cout << "free memory: " << free / 1024 / 1024 << "mb, total memory: " << total / 1024 / 1024 << "mb" << endl;  */
    
       // cout<<"Return Float :"<<returnFloat[i]<<endl;
       // cvShowImage("Object",this->descriptorsObject);
       
        matchesFB.clear();
        imageKeyPointsGpuMat.~GpuMat();
        imageDescriptorsGpuMat.~GpuMat();
        
        
        /*result = cuMemGetInfo(&free, &total);  
         cout << "free memory After release: " << free / 1024 / 1024 << "mb, total memory: " << total / 1024 / 1024 << "mb" << endl;  */
     
    }

   /* matcher.add( descCollection );
   // matcher.train();
   matcher.makeGpuCollection(trainCollection, cv::gpu::GpuMat(), vector<cv::gpu::GpuMat>());
    matcher.matchCollection(objectDescriptorsGpuMat, trainCollection, trainIdx, imgIdx, distance, cv::gpu::GpuMat());
    //matcher.matchDownload(trainIdx, distance, matchesOfTwo);
    matcher.matchDownload(trainIdx,imgIdx,distance, matchesOfTwo);
     //fbMatcher->match(cv::Mat(descriptorsObject),matchesOfTwo,std::vector<Mat>());
     
     cout<<endl;
    for(int j=0;j<matchesOfTwo.size();j++)
    {
        cout<<matchesOfTwo[j].distance<<" ";
    }         
    cout<<endl;    */
    
    objectKeyPointsGpuMat.~GpuMat();
	objectDescriptorsGpuMat.~GpuMat();
	
    return returnFloat;

}
float SURFeatures::matchSURFGPU(IplImage* imageTwo)
{    
     //IplImage* imageTwo = dbConnect->loadImageById(id);
     float   returnFloat=-1;
     
     if( imageTwo!=0)
     {
           setImage(imageTwo);
           extractSURFGPU(1);
           
          int minNoKeypoints=1;
          int maxNoKeypoints=1;
      
          if(((cv::Mat)objectDescriptorsGpuMat).rows < ((cv::Mat)imageDescriptorsGpuMat).rows)
          {
               matcherFB.match( (cv::Mat)objectDescriptorsGpuMat,(cv::Mat)imageDescriptorsGpuMat, matchesFB );
               // matcherFB.knnMatch( (cv::Mat)objectDescriptorsGpuMat,(cv::Mat)imageDescriptorsGpuMat, matchesFB );
                minNoKeypoints = ((cv::Mat)objectDescriptorsGpuMat).rows;
                maxNoKeypoints = ((cv::Mat)imageDescriptorsGpuMat).rows;
                
          }
          else
          {
               // matcherFB.match( (cv::Mat)imageDescriptorsGpuMat,(cv::Mat)objectDescriptorsGpuMat, matchesFB );
                matcherFB.match( (cv::Mat)imageDescriptorsGpuMat,(cv::Mat)objectDescriptorsGpuMat, matchesFB );
                 minNoKeypoints = ((cv::Mat)imageDescriptorsGpuMat).rows;
                maxNoKeypoints = ((cv::Mat)objectDescriptorsGpuMat).rows;
          }
          
          /*double max_dist = 0; double min_dist = 100;
           for( int i = 0; i < minNoKeypoints; i++ )
           { 
                double dist = matchesFB[i].distance;
                if( dist < min_dist ) min_dist = dist;
                if( dist > max_dist ) max_dist = dist;
           }*/
           
           int goodMatchCount =0; 
           
           //vector< DMatch > good_matches; 
          // vector<double> goodDistances;

          double sumDistance = 0;
          for( int p = 0; p < minNoKeypoints;p++ )
          {   
           // if( matchesFB[p].distance <2*min_dist )
            {
            
                sumDistance = sumDistance + matchesFB[p].distance;
               // good_matches.push_back(matchesFB[p]);
              //  goodDistances.push_back(matchesFB[p].distance);
                goodMatchCount++;          
            }
            
           
            
          }  
          
          
          
        //  std::sort(goodDistances.begin(), goodDistances.end());
           returnFloat=-1;
          if(goodMatchCount>0)
          {
          
            returnFloat = sumDistance/(float)goodMatchCount;
            /*Mat img_matches;
            
            if(((cv::Mat)objectDescriptorsGpuMat).rows < ((cv::Mat)imageDescriptorsGpuMat).rows)
            {
                 drawMatches(  Mat(this->object), keypointsObject,Mat(this->image), keypointsImage,  good_matches, img_matches, Scalar::all(-1), Scalar::all(-1), vector<char>(), DrawMatchesFlags::NOT_DRAW_SINGLE_POINTS );
            }
            else
           {
                //  drawMatches(Mat(this->image), keypointsObject, Mat(this->object), keypointsImage, matchesOfTwo, img_matches);
                drawMatches( Mat(this->image), keypointsImage, Mat(this->object), keypointsObject,  good_matches, img_matches, Scalar::all(-1), Scalar::all(-1), vector<char>(), DrawMatchesFlags::NOT_DRAW_SINGLE_POINTS );
            } 
        
           // namedWindow("matches", 0);
            //imshow("matches", img_matches); .
            char buf[50];
            sprintf(buf,"output\\%.4f_%d.jpg", returnFloat,imageTwo->ID);
          //cvSaveImage(buf,imageTwo,0); 
           // imwrite(buf,img_matches);     */
            cvReleaseImage(&imageTwo);
           
           // cvWaitKey(0); 
           // img_matches.~Mat();
        }
         
         //int midPoint = goodMatchCount/2;
          //returnFloat = goodMatchCount/(float)minNoKeypoints;
        //  returnFloat = sumDistance/(float)goodMatchCount;
         /*if( goodMatchCount>0)
         {
            returnFloat = (float)goodDistances[midPoint];
          }else
          {
             returnFloat =-1;
          }*/
          
          matchesFB.clear();
          imageKeyPointsGpuMat.~GpuMat();
          imageDescriptorsGpuMat.~GpuMat();
          
          
     }
     
    // objectKeyPointsGpuMat.~GpuMat();
	 //objectDescriptorsGpuMat.~GpuMat();
	
     return returnFloat;
     
   

}

/*
int naiveNearestNeighbor( const float* vec, int laplacian, const CvSeq* model_keypoints, const CvSeq* model_descriptors )
{
    int length = (int)(model_descriptors->elem_size/sizeof(float));
    int i, neighbor = -1;
    double d, dist1 = 1e6, dist2 = 1e6;
    CvSeqReader reader, kreader;
    cvStartReadSeq( model_keypoints, &kreader, 0 );
    cvStartReadSeq( model_descriptors, &reader, 0 );

    for( i = 0; i < model_descriptors->total; i++ )
    {
        const CvSURFPoint* kp = (const CvSURFPoint*)kreader.ptr;
        const float* mvec = (const float*)reader.ptr;
    	CV_NEXT_SEQ_ELEM( kreader.seq->elem_size, kreader );
        CV_NEXT_SEQ_ELEM( reader.seq->elem_size, reader );
        if( laplacian != kp->laplacian )
            continue;
        d = this->compareSURFDescriptors( vec, mvec, dist2, length );
        if( d < dist1 )
        {
            dist2 = dist1;
            dist1 = d;
            neighbor = i;
        }
        else if ( d < dist2 )
            dist2 = d;
    }
    if ( dist1 < 0.6*dist2 )
        return neighbor;
    return -1;
}*/
  /*
void findPairs( const CvSeq* objectKeypoints, const CvSeq* objectDescriptors, const CvSeq* imageKeypoints, const CvSeq* imageDescriptors, vector<int>& ptpairs )
{
    int i;
    CvSeqReader reader, kreader;
    cvStartReadSeq( objectKeypoints, &kreader );
    cvStartReadSeq( objectDescriptors, &reader );
    ptpairs.clear();

    for( i = 0; i < objectDescriptors->total; i++ )
    {
        const CvSURFPoint* kp = (const CvSURFPoint*)kreader.ptr;
        const float* descriptor = (const float*)reader.ptr;
        CV_NEXT_SEQ_ELEM( kreader.seq->elem_size, kreader );
        CV_NEXT_SEQ_ELEM( reader.seq->elem_size, reader );
        int nearest_neighbor = naiveNearestNeighbor( descriptor, kp->laplacian, imageKeypoints, imageDescriptors );
        if( nearest_neighbor >= 0 )
        {
            ptpairs.push_back(i);
            ptpairs.push_back(nearest_neighbor);
        }
    }
}   */
float SURFeatures::dpMatchSURFGPUCPU(vector<float> descriptorsObject,vector<float> descriptorsImage)

{
    /*     float* descriptorsObjectArray = &descriptorsObject[0];
         float* descriptorsImageArray  = &descriptorsImage[0];*/
         
         float* descriptorsMinArray;
         float* descriptorsMaxArray;
         int minSize,maxSize;
         
         /*minSize = 2*128;
         maxSize = 10*128;*/
         
      
         
          
         
         
         
         if(descriptorsObject.size()<descriptorsImage.size())
         {
            descriptorsMinArray= &descriptorsObject[0];
            descriptorsMaxArray= &descriptorsImage[0];
            
            minSize=descriptorsObject.size();
            maxSize=descriptorsImage.size(); 
         }
         else
         {
            descriptorsMinArray= &descriptorsImage[0];
            descriptorsMaxArray= &descriptorsObject[0];
            
            minSize=descriptorsImage.size();
            maxSize=descriptorsObject.size(); 
         }    
         
       /*  descriptorsMinArray = (float*)malloc(sizeof(float)*minSize);
         descriptorsMaxArray = (float*)malloc(sizeof(float)*maxSize);
         
         for(int i=0;i<minSize;i++)
         {
            /*if(i<128)
            {
                 descriptorsMinArray[i] = 2;
            }
            else* /
            {
                descriptorsMinArray[i] = i%128;         
            }
         }
         for(int i=0;i<maxSize;i++)
         {
            /*if(i<128)
            {
                 descriptorsMaxArray[i] = 2;
            }
            else* /
            {
                descriptorsMaxArray[i] = i;         
            }
         }  */
         
        /* float* descriptorsObjectArray  = (float*)malloc(sizeof(float)*1*128);
         float* descriptorsImageArray  = (float*)malloc(sizeof(float)*2*128);*/
         
         /*for(int i=30000;i<descriptorsObject.size();i++)
         {
               // descriptorsObjectArray[i]= descriptorsObject[i];
               // descriptorsObjectArray[i]= 1;
                cout<<"Object : "<<i<<": "<< descriptorsObjectArray[i]<<endl;
         }
         
         for(int i=0;i<2*128;i++)
         {
                //descriptorsImageArray[i]= descriptorsObject[i];
               // descriptorsImageArray[i]= 2;
                 cout<<" Image : "<<i<<": "<< descriptorsImageArray[i]<<endl;
         } 

            */

        // euclideanDistance(descriptorsObjectArray,descriptorsObject.size(),descriptorsImageArray, descriptorsImage.size());
        float dpdistance = getDPDistance(descriptorsMinArray,minSize,descriptorsMaxArray,maxSize);
         //euclideanDistance(descriptorsObjectArray,128,descriptorsImageArray, 256);
         
         /*for(int i=0;i<descriptorsObject.size();i++)
         {
            cout<<endl<<descriptorsObject[i]<<endl;
         }*/
         
        /* delete  descriptorsMinArray;
         delete  descriptorsMaxArray;*/
         
         return dpdistance;
         //return 10;
}

void SURFeatures::flannFindPairs(vector<int>& ptpairs )
{
	int length = (int)(objectDescriptors->elem_size/sizeof(float));

    cv::Mat m_object(objectDescriptors->total, length, CV_32F);
	cv::Mat m_image(imageDescriptors->total, length, CV_32F);


	// copy descriptors
    CvSeqReader obj_reader;
	float* obj_ptr = m_object.ptr<float>(0);
    cvStartReadSeq( objectDescriptors, &obj_reader );
    for(int i = 0; i < objectDescriptors->total; i++ )
    {
        const float* descriptor = (const float*)obj_reader.ptr;
        CV_NEXT_SEQ_ELEM( obj_reader.seq->elem_size, obj_reader );
        memcpy(obj_ptr, descriptor, length*sizeof(float));
        obj_ptr += length;
    }
    CvSeqReader img_reader;
	float* img_ptr = m_image.ptr<float>(0);
    cvStartReadSeq( imageDescriptors, &img_reader );
    for(int i = 0; i < imageDescriptors->total; i++ )
    {
        const float* descriptor = (const float*)img_reader.ptr;
        CV_NEXT_SEQ_ELEM( img_reader.seq->elem_size, img_reader );
        memcpy(img_ptr, descriptor, length*sizeof(float));
        img_ptr += length;
    }

    // find nearest neighbors using FLANN
    cv::Mat m_indices(objectDescriptors->total, 2, CV_32S);
    cv::Mat m_dists(objectDescriptors->total, 2, CV_32F);
    cv::flann::Index flann_index(m_image, cv::flann::KDTreeIndexParams(4));  // using 4 randomized kdtrees
    flann_index.knnSearch(m_object, m_indices, m_dists, 2, cv::flann::SearchParams(64) ); // maximum number of leafs checked

    int* indices_ptr = m_indices.ptr<int>(0);
    float* dists_ptr = m_dists.ptr<float>(0);
    for (int i=0;i<m_indices.rows;++i) {
    	if (dists_ptr[2*i]<0.6*dists_ptr[2*i+1]) {
    		ptpairs.push_back(i);
    		ptpairs.push_back(indices_ptr[2*i]);
    	}
    }
}


/* a rough implementation for object location */
int SURFeatures::locatePlanarObject()
{
    double h[9];
    CvMat _h = cvMat(3, 3, CV_64F, h);
    vector<int> ptpairs;
    vector<CvPoint2D32f> pt1, pt2;
    CvMat _pt1, _pt2;
    int i, n;

#ifdef USE_FLANN
    flannFindPairs( ptpairs );
#else
    findPairs( objectKeypoints, objectDescriptors, imageKeypoints, imageDescriptors, ptpairs );
#endif

    n = ptpairs.size()/2;
    if( n < 4 )
        return 0;

    pt1.resize(n);
    pt2.resize(n);
    for( i = 0; i < n; i++ )
    {
        pt1[i] = ((CvSURFPoint*)cvGetSeqElem(objectKeypoints,ptpairs[i*2]))->pt;
        pt2[i] = ((CvSURFPoint*)cvGetSeqElem(imageKeypoints,ptpairs[i*2+1]))->pt;
    }

    _pt1 = cvMat(1, n, CV_32FC2, &pt1[0] );
    _pt2 = cvMat(1, n, CV_32FC2, &pt2[0] );
    if( !cvFindHomography( &_pt1, &_pt2, &_h, CV_RANSAC, 5 ))
        return 0;

    for( i = 0; i < 4; i++ )
    {
        double x = src_corners[i].x, y = src_corners[i].y;
        double Z = 1./(h[6]*x + h[7]*y + h[8]);
        double X = (h[0]*x + h[1]*y + h[2])*Z;
        double Y = (h[3]*x + h[4]*y + h[5])*Z;
        dst_corners[i] = cvPoint(cvRound(X), cvRound(Y));
    }

    return 1;
}

double SURFeatures::compareSURFDescriptors( const float* d1, const float* d2, double best, int length )
{
    double total_cost = 0;
    assert( length % 4 == 0 );
    for( int i = 0; i < length; i += 4 )
    {
        double t0 = d1[i] - d2[i];
        double t1 = d1[i+1] - d2[i+1];
        double t2 = d1[i+2] - d2[i+2];
        double t3 = d1[i+3] - d2[i+3];
        total_cost += t0*t0 + t1*t1 + t2*t2 + t3*t3;
        if( total_cost > best )
            break;
    }
    return total_cost;
}
SURFeatures::~SURFeatures(void)
{
   //keypointsObject.clear();
    //descriptorsObject.clear();
    
  //  cvReleaseMemStorage(&storage);
	/*objectKeyPointsGpuMat.~GpuMat();
	objectDescriptorsGpuMat.~GpuMat();
	imageKeyPointsGpuMat.~GpuMat();
	imageDescriptorsGpuMat.~GpuMat(); */

	 //params = cvSURFParams(500, 1);
    
}